#!/usr/bin/env python3
# coding=utf-8

import sys
from PyQt5.QtWidgets import QApplication, QMainWindow
from PyQt5.QtCore import QObject, pyqtSignal, Qt
from PyQt5.QtGui import QPixmap, QImage
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QLabel
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import String
from threading import Thread
import rospy
from pid_msgs.msg import param_pid as ParamMsg  # 确保导入正确的消息类型
from Ui_qttest1 import Ui_Form  # 导入转换后的 UI 类
import cv2
from cv_bridge import CvBridge
import time
from pid_msgs.msg import pub_param as PubMsg
from pid_msgs.msg import pub_stop_param as StopMsg   
import keyboard
from threading import Thread, Lock
import os
from pid_msgs.msg import point_center
from pid_msgs.srv import Num_params, Num_paramsRequest
from pid_msgs.msg import Show_param
from PyQt5.QtCore import QRegExp
from PyQt5.QtGui import QRegExpValidator
import math
import json

class MainWindow(QMainWindow, Ui_Form):
    def __init__(self):
        super().__init__()

        # self.turn_data = {
        #     "弯道1": {"corner_in": 1, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道2": {"corner_in": 2, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道3": {"corner_in": 3, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道4": {"corner_in": 4, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道5": {"corner_in": 5, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道6": {"corner_in": 6, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道7": {"corner_in": 7, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
        #     "弯道8": {"corner_in": 8, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2}
            
        # }
        

        self.data_file = "//home/qiao/face_ws/src/face/turn_data_12_2.json"
        self.turn_data = self.load_data()

        self.setupUi(self)  # 使用转换后的 UI
        self.setFixedSize(self.width(), self.height())
        # self.setFocusPolicy(QtCore.Qt.NoFocus)
        self.init_ros()
        self.show()


    def init_ros(self):
        """初始化ROS监听器"""
        self.ros_listener = ROSListener()

        self.pub_param = rospy.Publisher("/set_param", PubMsg, queue_size=10)
        self.stop_param = rospy.Publisher("/stop_param", StopMsg, queue_size=10)

        # 连接信号和UI的槽
        self.ros_listener.params_updated.connect(self.update_params)
        self.ros_listener.image_updated.connect(self.update_image_frame3)
        self.ros_listener.mask_updated.connect(self.update_image_frame4)

        # 连接按钮
        self.pushButton_D_add.clicked.connect(lambda: self.increment_value(self.lineEdit_D))
        self.pushButton_I_add.clicked.connect(lambda: self.increment_value(self.lineEdit_I))
        self.pushButton_P_add.clicked.connect(lambda: self.increment_value(self.lineEdit_P))
        self.pushButton_dx_add.clicked.connect(lambda: self.increment_value(self.lineEdit_default_speed))
        self.pushButton_w_add.clicked.connect(lambda: self.increment_value(self.lineEdit_w_width))

        self.pushButton_D_sub.clicked.connect(lambda: self.decrement_value(self.lineEdit_D))
        self.pushButton_I_sub.clicked.connect(lambda: self.decrement_value(self.lineEdit_I))
        self.pushButton_P_sub.clicked.connect(lambda: self.decrement_value(self.lineEdit_P))
        self.pushButton_dx_sub.clicked.connect(lambda: self.decrement_value(self.lineEdit_default_speed))
        self.pushButton_w_sub.clicked.connect(lambda: self.decrement_value(self.lineEdit_w_width))


        # 文本框数据编辑完成后，保存下来
        self.lineEdit_corner_in.editingFinished.connect(self.save_turn_data)
        self.lineEdit_corner_turn.editingFinished.connect(self.save_turn_data)
        self.lineEdit_corner_z.editingFinished.connect(self.save_turn_data)
        self.lineEdit_corner_x.editingFinished.connect(self.save_turn_data)


        # 弯道信息
        self.combo_corner.addItems(self.turn_data.keys())
        # 为下拉框添加槽函数
        self.combo_corner.currentIndexChanged.connect(self.update_turn_data)
        self.update_turn_data()
        # 为应用按钮设置一个点击事件：当应用时，将PID参数，颜色，初始线速度发布出去
        self.pushButton_setting.clicked.connect(self.pub_params)

        # 为地图样式添加一个点击事件
        self.map_button.clicked.connect(self.show_map_dialog)

        # 为暂停按钮添加点击事件
        # self.pushButton_stop.clicked.connect(self.stop)

        # 加载json文件保存的数据


        # 启动ROS线程
        self.ros_thread = Thread(target=self.ros_listener.spin)
        self.ros_thread.start()
    
    def show_map_dialog(self):
        pixmap = QtGui.QPixmap("/home/qiao/Desktop/map.jpg")
        self.label_map.setPixmap(pixmap)
        self.dialog.show()


    # 下拉框的槽函数
    def update_turn_data(self):
        current_text = self.combo_corner.currentText()
        if current_text in self.turn_data:
            self.lineEdit_corner_in.setText(str(self.turn_data[current_text]['corner_in']))
            self.lineEdit_corner_turn.setText(str(self.turn_data[current_text]['corner_turn']))
            self.lineEdit_corner_z.setText(str(self.turn_data[current_text]['corner_z']))
            self.lineEdit_corner_x.setText(str(self.turn_data[current_text]['corner_x']))

    # 保存文本框中的数据到字典中
    def save_turn_data(self):
        current_Text = self.combo_corner.currentText()
        if current_Text in self.turn_data:
            self.turn_data[current_Text]['corner_in'] = self.lineEdit_corner_in.text()
            self.turn_data[current_Text]['corner_turn'] = self.lineEdit_corner_turn.text()
            self.turn_data[current_Text]['corner_z'] = self.lineEdit_corner_z.text()
            self.turn_data[current_Text]['corner_x'] = self.lineEdit_corner_x.text()

    def save_data(self):
        try:
            with open(self.data_file, "w", encoding="utf-8") as f:
                json.dump(self.turn_data, f, ensure_ascii=False, indent=4)
            print("数据已保存到文件")
        except Exception as e:
            print("保存数据失败: {}".format(e))


    def load_data(self):
        """从 JSON 文件加载 turn_data,如果失败则返回默认值"""
        try:
            with open(self.data_file, "r", encoding="utf-8") as f:
                return json.load(f)
        except FileNotFoundError:
            # 如果文件不存在，返回默认值
            print("数据文件不存在，使用默认值。")
            return {
                "弯道1": {"corner_in": 1, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道2": {"corner_in": 2, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道3": {"corner_in": 3, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道4": {"corner_in": 4, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道5": {"corner_in": 5, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道6": {"corner_in": 6, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道7": {"corner_in": 7, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2},
                "弯道8": {"corner_in": 8, "corner_turn": 2.5, "corner_z": 0.7, "corner_x": 0.2}
            }
        except Exception as e:
            print("加载数据失败：{}".format(e))
            return {}

    def update_image_frame3(self, qpixmap):
        # 更新图像显示
        self.label_image_frame3.setPixmap(qpixmap)
        self.label_image_frame3.setScaledContents(True)

    def update_image_frame4(self, qpixmap):
        # 更新frame4中的图像
        self.label_image_frame4.setPixmap(qpixmap)
        self.label_image_frame4.setScaledContents(True)

    def update_params(self, msg):
        """
        更新标签和文本框内容
        """
        # self.lineEdit_P.setText(f"{msg.P:.2f}")
        # self.lineEdit_I.setText(f"{msg.I:.2f}")
        # self.lineEdit_D.setText(f"{msg.D:.2f}")
        # self.lineEdit_color.setText(msg.color)
        # self.lineEdit_default_speed.setText(f"{msg.c_x:.2f}")
        self.label_cx.setText(f"{msg.c_x:.2f} m/s")
        self.label_cz.setText(f"{msg.c_z:.2f} rad/s")
        self.label_error.setText(f"{msg.error:.2f} 像素点")

        format_time = time.strftime('%H:%M:%S', time.gmtime(msg.time))

        self.label_time.setText(f"{format_time}")
        avg_slope = msg.slope
        # print("avg_slope: {}".format(avg_slope))
        trend = "unknow"
        if avg_slope == 0:  # 应该按照角速度判断
            trend = "unknow"
        else:

            if avg_slope > 0.5:
                
                trend = "Left Turn"
            
            elif avg_slope < -0.5:
            
                trend = "Right Turn"
            
            else:
            
                trend = "Straight"
        self.label_turn_str.setText(f"{trend}")
           


    # def stop(self):
    #     try:
    #         msg = StopMsg()
    #         msg.x = 0.0
    #         msg.z = 0.0
    #         self.stop_param.publish(msg)
    #     except Exception as e:
    #         print("Error: stop failed :{}".format(e))
        
        

    # 对按钮添加点击事件 +  
    def increment_value(self, line_edit):
        # 获取当前文本框中的值
        current_value = line_edit.text()
        try:
            # 将值转换为浮动数并增加 0.01
            new_value = float(current_value) + 0.01
            # 更新文本框的值
            line_edit.setText(str(round(new_value, 2)))  # 保留两位小数
        except ValueError:
            # 如果文本框不是数字，忽略此操作
            pass
    # -
    def decrement_value(self, line_edit):
        # 获取当前文本框中的值
        current_value = line_edit.text()
        try:
            # 将值转换为浮动数并减少 0.001
            new_value = float(current_value) - 0.001
            # 更新文本框的值
            line_edit.setText(str(round(new_value, 3)))  # 保留三位小数
        except ValueError:
            # 如果文本框不是数字，忽略此操作
            pass

    def get_corner_data(self):
        """从 self.turn_data 提取弯道信息"""
        corner_in = [float(self.turn_data[key]["corner_in"]) for key in self.turn_data]
        corner_turn = [float(self.turn_data[key]["corner_turn"]) for key in self.turn_data]
        corner_z = [float(self.turn_data[key]["corner_z"]) for key in self.turn_data]
        corner_x = [float(self.turn_data[key]["corner_x"]) for key in self.turn_data]



        return corner_in, corner_turn, corner_z, corner_x


    def pub_params(self):

        self.save_data()

        rospy.wait_for_service("/Num_params")
        try:
            corner_in, corner_turn, corner_z, corner_x = self.get_corner_data()
            # 获取用户输入的值并进行检查
            P = self.lineEdit_P.text()
            I = self.lineEdit_I.text()
            D = self.lineEdit_D.text()
            windows_width = self.lineEdit_w_width.text()
            start_x = self.lineEdit_default_speed.text()
            

            # 验证输入的值
            if not self.is_valid_input(P) or not self.is_valid_input(I) or not self.is_valid_input(D):
                print("Error: Please enter valid numerical values for P, I, and D (greater than 0).")
                return
            
            if not self.is_valid_integer_input(windows_width):
                print("Error: Please enter valid positive integers for len_q and windows_width.")
                return
            
            if not self.is_valid_input(start_x):
                print("Error: Please enter a valid positive number for start_x.")
                return
          

            # 创建服务请求
            srv = Num_paramsRequest()
            srv.P = float(P)
            srv.I = float(I)
            srv.D = float(D)
            srv.windows_width = int(windows_width)
            srv.start_x = float(start_x)
            srv.corner_in = corner_in
            srv.corner_turn = corner_turn
            srv.corner_z = corner_z
            srv.corner_x = corner_x
            print("corner_in: {}".format(corner_in))

            # 调用服务
            response = self.ros_listener.client(srv)
            # 保存数据
            
            if response:

                rospy.loginfo("请求成功！")
            
        except rospy.ServiceException as e:
            print(f"Error: failed to request params: {e}")

    def is_valid_input(self, value):
        """验证浮动数字是否有效且大于0"""
        try:
            num = float(value)
            return num > 0
        except ValueError:
            return False

    def is_valid_integer_input(self, value):
        """验证整数是否有效且大于0"""
        try:
            num = int(value)
            return num > 0
        except ValueError:
            return False

    def setup_line_edits(self):
        # 设置 QLineEdit 控件，只允许输入大于零的浮点数或整数
        float_regexp = QRegExp(r"^\d*\.?\d+$")  # 正则表达式，表示浮点数或整数
        int_regexp = QRegExp(r"^\d+$")  # 只允许整数

        # 设置正则表达式校验器
        float_validator = QRegExpValidator(float_regexp, self)
        int_validator = QRegExpValidator(int_regexp, self)

        # 应用到相关文本框
        self.lineEdit_P.setValidator(float_validator)
        self.lineEdit_I.setValidator(float_validator)
        self.lineEdit_D.setValidator(float_validator)
        #self.lineEdit_len_q.setValidator(int_validator)
        self.lineEdit_w_width.setValidator(int_validator)
        self.lineEdit_default_speed.setValidator(float_validator)



    def keyPressEvent(self, event):
        """重载键盘按下事件"""
        if event.key() == Qt.Key_Space:
            self.save_current_image()
        else:
            super(MainWindow, self).keyPressEvent(event)

    def save_current_image(self):
        """保存当前帧图像"""
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        self.ros_listener.save_current_image(timestamp)

    def closeEvent(self, event):
        """
        处理窗口关闭事件，确保ROS节点和线程被正确关闭
        """
        self.ros_listener.shutdown()
        self.ros_thread.join()
        event.accept()

class ROSListener(QObject):
    # 定义信号，用于传递ROS消息到GUI
    params_updated = pyqtSignal(object)
    # 定义传递图片的信号
    image_updated = pyqtSignal(QPixmap)
    # 定义传递mask的信号
    mask_updated = pyqtSignal(QPixmap)

    def __init__(self):
        super().__init__()
        rospy.init_node('ros_pyqt_display', anonymous=True)
        # rospy.Subscriber("/pid_params", ParamMsg, self.callback_param)
        rospy.Subscriber("/Show_param", Show_param, self.callback_param)
        # rospy.Subscriber("/face_image", Image, self.callback_image)
        rospy.Subscriber("/image_mask", Image, self.callback_mask)
        rospy.Subscriber("/point_center", point_center, self.callback_points)    

        # rospy.wait_for_service('/Num_params')
        self.client =  rospy.ServiceProxy("/Num_params", Num_params)

        # 设置保存图片的文件夹路径
        # self.save_folder = "saved_images_3"
        # if not os.path.exists(self.save_folder):
        #     os.makedirs(self.save_folder)

        self.bridge = CvBridge()
        self._running = True
        self.frame_count = 0 
        # self.lock = Lock()
        self.latest_image = None  # 存储最新的图像
        self.frame_count = 0
        self.last_time = time.time()

    def compute_slope(self, points):
        slopes = []
        avg_slope = 0
        angles = []
        for i in range(len(points)-1):
            delta_x = points[i][0] - points[i+1][0]
            delta_y = points[i][1] - points[i+1][1]

            if delta_y != 0:
                slope = delta_x / delta_y
                slopes.append(slope)
                # 计算角度
                if delta_x == 0:  # 如果 x 相等，角度设为 90 度
                    angle = 90
                else:
                    # 使用 arctan 计算角度并转换为度数
                    angle_rad = math.atan(slope)
                    angle = math.degrees(angle_rad)  # 弧度转为度数

                    # 如果斜率为负（顺时针旋转），将其转换为正角度
                    if angle < 0:
                        angle = -angle  # 转换为正角度

                    # 限制角度在 [0, 180) 范围内
                    if angle >= 180:
                        angle = 180 - angle
                angles.append(angle)
            else:
                slopes.append(0)
                angles.append(90)       
        
        return slopes, angles

    def callback_points(self, points):

        current_time = time.time()

        # 将ROS图像消息转换为OpenCV图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(points.image, "bgr8")
            point_a = []
            if points.p1:
                
                # 将点画上去 
                point_a.append((points.p1[0], points.p1[1]))
               
                point_a.append((points.p2[0], points.p2[1]))
                
                point_a.append((points.p3[0], points.p3[1]))
                
                point_a.append((points.p4[0], points.p4[1]))
            
                point_a.append((points.p5[0], points.p5[1]))

                for point in point_a:
                    x, y = int(point[0]), int(point[1])
                    if 0 <= x < cv_image.shape[1] and 0 <= y < cv_image.shape[0]:  # 防止越界
                        cv_image[y-1:y+1, x-1:x+1] = (255, 0, 0)  # 使用简单的矩阵操作绘制一个小圆点


                slopes, angles = self.compute_slope(points=point_a)

            # image = cv2.resize(cv_image, (80, 60))
            cv_image = cv2.resize(cv_image, (460, 300))
            
            # 将OpenCV BGR格式的图像转换为RGB格式  因为qt显示的rgb格式
            cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)

	    

            # 存储最新的图像
            # with self.lock:
                # self.latest_image = image.copy()
            # 转换为QPixmap
            height, width, channel = cv_image.shape
            bytes_per_line = 3 * width
            q_image = QPixmap(QImage(cv_image.data, width, height, bytes_per_line, QImage.Format_RGB888))
            self.image_updated.emit(q_image)  # 触发信号，传递QPixmap到UI
           
            self.frame_count += 1
            if current_time - self.last_time >= 1:
                # print("FPS: {}".format(self.frame_count))
                self.frame_count = 0
                self.last_time= current_time

            
        except Exception as e:
            print("Error converting image:", e)
            


    def callback_param(self, msg):
        # 通过信号将消息传递给GUI线程
        self.params_updated.emit(msg)


    

    def callback_mask(self, msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="mono8")
            height, width = cv_image.shape
            q_mask = QPixmap(QImage(cv_image.data, width, height, width, QImage.Format_Grayscale8))
            self.mask_updated.emit(q_mask)
        except Exception as e:
            print("Error converting mask:", e)


    def spin(self):
        # 使用rospy.spin()来处理回调
        rospy.spin()

    def shutdown(self):
        self._running = False
        rospy.signal_shutdown("Shutting down ROS Listener")

def main():
    try:
        print("启动 ROS 和 PyQt 界面")
        app = QApplication(sys.argv)
        window = MainWindow()
        window.show()
        sys.exit(app.exec_())
    except rospy.ROSInterruptException:
        print("ROS 中断异常")
    except Exception as e:
        print(f"发生异常: {e}")

if __name__ == '__main__':
    main()
